
import machine
import time
import math
import struct
from micropython import const

class MicroBlueError(Exception):
    pass


def my_uuid():
    uid = machine.unique_id()
    return ''.join(['{:02x}'.format(b) for b in uid])


class TtlMotor:
    def __init__(self, rx, tx):
        self.rx = rx
        self.tx = tx
        self.speed = 0
        self.angles = 0

    def turnOffTorque(self, uart):
        uart.write(b"\xF9\xFF\xFE\x04\x03\x64\x00\xCC")

    def turnOnTorque(self, uart):
        uart.write(b"\xF9\xFF\xFE\x04\x03\x64\x02\xCC")

    def servosMode(self, uart):
        uart.write(b"\xF9\xFF\x01\x04\x03\x5A\x01\xCC")

    def motorMode(self, uart):
        uart.write(b"\xF9\xFF\x01\x04\x03\x5A\x02\xCC")
        
    def close(self, uart):
        time.sleep(0.005)
        uart.deinit()
        machine.Pin(self.tx, machine.Pin.IN)
        machine.Pin(self.rx, machine.Pin.IN)

    def motor_run(self, speed, clockwise=True):
        """ 电机以指定速度运行，正转或反转 """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)

        self.turnOffTorque(uart)
        self.motorMode(uart)
        self.turnOnTorque(uart)

        if not clockwise:
            speed = 0 - speed

        self.speed = speed

        speed_bytes = int(speed).to_bytes(2, "little")
        uart.write(b"\xF9\xFF\x01\x05\x03\x6E" + speed_bytes + b"\xCC")
        self.close(uart)

    def motor_reset(self):
        """ 电机置零：停止运动 """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)
        self.turnOffTorque(uart)
        self.servosMode(uart)
        self.turnOnTorque(uart)
        uart.write(b"\xF9\xFF\x01\x04\x03\x1D\x01\xD9")
        self.close(uart)
        
        self.speed = 0
        self.angles = 0
        

    def motor_get_speed(self):
        """ 获取电机运行速度 """
        return self.speed
    
    def get_relative_angle(self):
        """ 获取相对角度 """
        return self.angles
    
    def get_absolute_angle(self):
        """ 获取绝对角度 """
        if self.angles > 0:
            return self.angles % 360
        elif self.angles < 0:
            return 360 - ((0-self.angles) % 360)
        else:
            return 0
        
    def get_circles(self):
        return round(self.angles / 360, 1)
        

    def servo_get_angle(self):
        """ 获取舵机角度(已废弃) """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)
        
        data = b''
        while True:
            uart.write(b"\xF9\xFF\x01\x03\x02\x46\xB3")
            time.sleep_ms(3)
            line = b''
            while uart.any():
                response = uart.read()
                line += response
            if len(line) >= 10 and line[0:2] == b"\xF9\xF5":
                data = line
                break
            
        if (
            len(data) >= 10
            and data[0:2] == b"\xF9\xF5"
            and data[3] == 0x07
            and data[4] == 0x02
            and data[5] == 0x46
        ):
            position_bytes = data[6:10]
            position = int.from_bytes(position_bytes, "little")
            if position > 2**31:  # 如果是负数
                position -= 2**32
            position_float = position / 100.0
            self.close(uart)
            return int(round(position/10))
        
        self.close(uart)
        return 0
        

    def motor_run_seconds(self, seconds, clockwise=True):
        """ 电机运行指定时间(秒) """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)
        
        self.turnOffTorque(uart)
        self.motorMode(uart)
        self.turnOnTorque(uart)

        seconds_value = int(seconds * 10)
        if seconds_value < 0:
            seconds_value += 2**16
        seconds_bytes = int(seconds_value).to_bytes(2, "little")
        direction = 0x14 if clockwise else 0x15
        command = (
            b"\xF9\xFF\x01\x09\x03\x70"
            + bytes([direction])
            + b"\x00"
            + seconds_bytes
            + b"\x84\x03\xCC"
        )
        uart.write(command)
        self.close(uart)

    def servo_relative_angle(self, angle):
        """ 舵机相对角度 """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)
        self.turnOffTorque(uart)
        self.servosMode(uart)
        self.turnOnTorque(uart)
        angle_bytes = int(angle * 10).to_bytes(4, "little")
        command = (
            b"\xF9\xFF\x01\x0D\x03\x67\x11\x00"
            + angle_bytes[:2]
            + angle_bytes[2:]
            + b"\x84\x03\x00\x00\xCC"
        )
        uart.write(command)
        self.close(uart)
        self.angles += angle

    def servo_absolute_angle(self, angle):
        """ 舵机绝对角度 """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)
        self.turnOffTorque(uart)
        self.servosMode(uart)
        self.turnOnTorque(uart)
        angle_value = int(angle * 10)
        if angle_value < 0:
            angle_value += 2**32
        angle_bytes = int(angle_value).to_bytes(4, "little")
        command = b"\xF9\xFF\x01\x07\x03\x65" + angle_bytes[:2] + b"\x00\x00\xCC"
        uart.write(command)
        self.close(uart)
        self.angles = angle

    def servo_relative_circles(self, circles):
        """ 舵机相对圈 """
        uart = machine.UART(1, tx=self.tx, rx=self.rx, baudrate=115200)
        self.turnOffTorque(uart)
        self.servosMode(uart)
        self.turnOnTorque(uart)

        degrees = int(circles * 3600)
        if degrees < 0:
            degrees += 2**32
        low_word = int(degrees & 0xFFFF).to_bytes(2, "little")
        high_word = int((degrees >> 16) & 0xFFFF).to_bytes(2, "little")
        command = (
            b"\xF9\xFF\x01\x0D\x03\x67\x11\x00"
            + low_word
            + high_word
            + b"\x84\x03\x00\x00\xCC"
        )
        uart.write(command)
        self.close(uart)
        self.angles += int(circles*360)

